
#pragma once

#include <rviz_common/tool.hpp>
#include <rviz_default_plugins/visibility_control.hpp>
#include <rviz_default_plugins/tools/interaction/interaction_tool.hpp>
#include <interactive_markers/interactive_marker_server.hpp>
#include "rviz_custom_plugins/utils/interactive_marker_thread.h"
#include "rviz_custom_plugins/utils/bag_map_notifier.h"
#include <pcl/octree/octree_search.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <pcl/features/normal_3d.h>
#include <QKeyEvent>


using rviz_default_plugins::tools::InteractionTool;
using rviz_custom_plugins::utils::BagMapNotifier;
using rviz_custom_plugins::utils::InteractiveMarkerThread;


namespace rviz_custom_plugins
{

namespace tools
{

class RVIZ_DEFAULT_PLUGINS_PUBLIC MapPointTool: public InteractionTool
{
    Q_OBJECT

public:
    MapPointTool();

    int processMouseEvent( rviz_common::ViewportMouseEvent& event ) override;

    void activate() override;
    void deactivate() override;
    void update(float wall_dt, float ros_dt) override;

    void load( const rviz_common::Config& config ) override;

signals:
    void pointCloudFiltered(pcl::PointCloud<pcl::PointXYZINormal>::Ptr pcdPointNomal);
    void mapPointSelected(QString nameSpace, int pclIndex, pcl::PointXYZINormal& point);

private:
    BagMapNotifier* mapNotifier;
    std::shared_ptr<InteractiveMarkerThread> markerThread;

    pcl::PointCloud<pcl::PointXYZINormal>::Ptr pcdPointNormal;
    pcl::PointCloud<pcl::PointXYZINormal>::Ptr pcdViewPointNormal;

    Ogre::Vector3 lastCameraPos;
    Ogre::Vector3 lastCameraDir;

    int findNearestPointToCamera(
        const Eigen::Vector3f& rayOrigin,
        const Eigen::Vector3f& rayDirection_normalized,
        float voxelResolution = 0.05f,
        float radiusSearch = 0.1f
    );

private Q_SLOTS:
    // void handleLoadPointCloudFinished(sensor_msgs::msg::PointCloud2::SharedPtr pcd, pcl::PointCloud<pcl::Normal>::Ptr normals);
    void handlePointCloudLoaded(pcl::PointCloud<pcl::PointXYZINormal>::Ptr pcdPointNomal);


};

}

}
